import random
from threading import local
import time
import numpy as np
from isaacgym import gymutil
from isaacgym import gymapi
#from isaacgym import gymtorch
from math import sqrt
import math
from omegaconf import read_write
from sympy import false, re
import torch
import cv2
import gym
from gym import spaces

from draw import *

from pcgworker.PCGWorker import *

from stable_baselines3.common.env_checker import check_env

# https://stable-baselines3.readthedocs.io/en/master/guide/custom_env.html

N_DISCRETE_ACTIONS = 6
N_CHANNELS = 3
HEIGHT = 84
WIDTH = 84

class CustomEnv(gym.Env):
    """Custom Environment that follows gym interface"""
    metadata = {'render.modes': ['human', 'rgb_array']}

    def __init__(self, headless_: bool = True, seed_pth: str = "blank_wave.json", render_indicator: bool = True):
        super(CustomEnv, self).__init__()
        # Define action and observation space
        self.action_space = spaces.Discrete(N_DISCRETE_ACTIONS)
        self.observation_space = spaces.Box(low=0, high=255, shape=(HEIGHT, WIDTH, N_CHANNELS), dtype=np.uint8)

        torch.manual_seed(3407)

        '''
        simulatiom parameters
        '''
        self.num_matrix_envs = 16
        self.headless = headless_
        self.spd = 10

        self.render_indicator = render_indicator

        self.initial_height = 11

        # get default set of parameters
        sim_params = gymapi.SimParams()
        args = gymutil.parse_arguments()
        sim_params.physx.solver_type = 1
        sim_params.physx.num_position_iterations = 2
        sim_params.physx.num_velocity_iterations = 1
        sim_params.physx.num_threads = args.num_threads
        sim_params.physx.use_gpu = args.use_gpu
        sim_params.use_gpu_pipeline = False #args.use_gpu_pipeline
        sim_params.substeps = 2
        sim_params.dt = 1.0 / 30.0
        # setting up the Z-up axis
        sim_params.up_axis = gymapi.UP_AXIS_Z
        sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.8)

        # configure the ground plane
        plane_params = gymapi.PlaneParams()
        plane_params.normal = gymapi.Vec3(0, 0, 1) # z-up!
        plane_params.distance = 1
        plane_params.static_friction = 1
        plane_params.dynamic_friction = 1
        plane_params.restitution = 0.1

        # set up the env grid
        num_envs = 1
        spacing = 20
        env_lower = gymapi.Vec3(-spacing, -spacing, -spacing)
        env_upper = gymapi.Vec3(spacing, spacing, spacing)

        # Attractor setup
        attractor_properties = gymapi.AttractorProperties()
        attractor_properties.stiffness = 5e10
        attractor_properties.damping = 0
        attractor_properties.axes = gymapi.AXIS_ROTATION
        attractor_pose = gymapi.Transform()
        attractor_pose.p = gymapi.Vec3(0, 0.0, 0.0)
        attractor_pose.r = gymapi.Quat(0,1,0,1)

        # create procedural asset
        asset_options = gymapi.AssetOptions()
        asset_options.density = 1.0
        asset_options.fix_base_link = False
        asset_options.linear_damping = 0.1
        asset_options1 = gymapi.AssetOptions()
        asset_options1.density = 0.02
        asset_options1.fix_base_link = True
        asset_options1.linear_damping = 0.1

        color_red = gymapi.Vec3(1,0,0)
        color_green = gymapi.Vec3(0,1,0)

        # load PCG asset
        asset_root = "./assets"
        pt_gray_cube = "PCG/gray_cube.urdf"
        pt_blue_cube = "PCG/blue_cube.urdf"
        pt_red_cube = "PCG/red_cube.urdf"
        pt_white_cube = "PCG/white_cube.urdf"
        pt_yellow_cube = "PCG/yellow_cube.urdf"
        pt_blue_ramp = "PCG/blue_ramp.urdf"
        pt_red_ramp = "PCG/red_ramp.urdf"
        pt_white_ramp = "PCG/white_ramp.urdf"
        pt_yellow_ramp = "PCG/yellow_ramp.urdf"
        pt_blue_corner = "PCG/blue_corner.urdf"
        pt_red_corner = "PCG/red_corner.urdf"
        pt_white_corner = "PCG/white_corner.urdf"
        pt_yellow_corner = "PCG/yellow_corner.urdf"

        # camera sensor properties
        camera_properties = gymapi.CameraProperties()
        camera_properties.width = WIDTH
        camera_properties.height = HEIGHT
        camera_properties.enable_tensors = True

        # rigid shape material properties
        shape_props = [gymapi.RigidShapeProperties()]
        shape_props[0].friction = -1
        shape_props[0].rolling_friction = 1
        shape_props[0].torsion_friction = 1
        shape_props[0].compliance = 0
        shape_props[0].restitution = 0
        shape_props1 = [gymapi.RigidShapeProperties()]
        shape_props1[0].friction = 0.2
        shape_props1[0].rolling_friction = -1
        shape_props1[0].torsion_friction = -1
        shape_props1[0].compliance = 0
        shape_props1[0].restitution = 0
        shape_props2 = [gymapi.RigidShapeProperties()]
        shape_props2[0].friction = 1
        shape_props2[0].rolling_friction = -1
        shape_props2[0].torsion_friction = -1
        shape_props2[0].compliance = 0
        shape_props2[0].restitution = 0

        # single world camera image
        self.camera_sensor_image = np.zeros((HEIGHT,WIDTH, 3), np.uint8)
        # human control
        self.interaction = 0

        # proximity threshold for reward calculation
        self.proximity_threshold = 1.5
        self.max_steps = 1000

        self.show_action = False

        '''
        generate WFC maps
        '''
        PCGWorker_ = PCGWorker(9,9)

        seed = PCGWorker_.from_file(filename = seed_pth)

        # seed_ = PCGWorker_.from_file(filename = "blank_wave.json")
        # seed = PCGWorker_.mutate(seed_,81)
        # PCGWorker_.to_file(seed,filename = "seed.json")

        # seed = PCGWorker_.generate()
        self.connectivity_map , self.connectivity_map_visual = PCGWorker_.connectivity_analysis(seed,visualize_ = False)

        seeds = [seed]
        for i in range(0,self.num_matrix_envs-1):
            seed_ = PCGWorker_.mutate(seeds[-1],81)
            seeds.append(seed_)

        self.gym = gymapi.acquire_gym()

        self.sim = self.gym.create_sim(args.compute_device_id, args.graphics_device_id, gymapi.SIM_PHYSX, sim_params)

        print("args.graphics_device_id : ",args.graphics_device_id)
        print("args.compute_device_id : ",args.compute_device_id)


        # create the ground plane
        self.gym.add_ground(self.sim, plane_params)

        capsule_asset = self.gym.create_capsule(self.sim, 1, 1, asset_options)
        # capsule_asset = self.gym.create_box(self.sim, 1, 1, 1, asset_options)

        asset_gray_cube = self.gym.load_asset(self.sim, asset_root, pt_gray_cube,asset_options1)
        asset_blue_cube = self.gym.load_asset(self.sim, asset_root, pt_blue_cube,asset_options1)
        asset_red_cube = self.gym.load_asset(self.sim, asset_root, pt_red_cube,asset_options1)
        asset_white_cube = self.gym.load_asset(self.sim, asset_root, pt_white_cube,asset_options1)
        asset_yellow_cube = self.gym.load_asset(self.sim, asset_root, pt_yellow_cube,asset_options1)
        asset_blue_ramp = self.gym.load_asset(self.sim, asset_root, pt_blue_ramp,asset_options1)
        asset_red_ramp = self.gym.load_asset(self.sim, asset_root, pt_red_ramp,asset_options1)
        asset_white_ramp = self.gym.load_asset(self.sim, asset_root, pt_white_ramp,asset_options1)
        asset_yellow_ramp = self.gym.load_asset(self.sim, asset_root, pt_yellow_ramp,asset_options1)
        asset_blue_corner = self.gym.load_asset(self.sim, asset_root, pt_blue_corner,asset_options1)
        asset_red_corner = self.gym.load_asset(self.sim, asset_root, pt_red_corner,asset_options1)
        asset_white_corner = self.gym.load_asset(self.sim, asset_root, pt_white_corner,asset_options1)
        asset_yellow_corner = self.gym.load_asset(self.sim, asset_root, pt_yellow_corner,asset_options1)

        if not self.headless:
            cam_props = gymapi.CameraProperties()
            self.viewer = self.gym.create_viewer(self.sim, cam_props)

        self.envs = []
        self.camera_handles = []
        self.actor_handles = []
        self.food_handles = []
        self.attractor_handles = []

        '''
        actor handles for all building blocks
        [
            env1:(l1_active_id,[l1_handles]),
            env2:(l1_active_id,[l1_handles]),
            ...
        ]
        '''
        self.gray_cube_handles = []         
        self.blue_cube_handles = []         
        self.blue_corner_handles = []       
        self.blue_ramp_handles = []             
        self.red_cube_handles = []
        self.red_corner_handles = []
        self.red_ramp_handles = []
        self.white_cube_handles = []
        self.white_corner_handles = []
        self.white_ramp_handles = []
        self.yellow_cube_handles = []
        self.yellow_corner_handles = []
        self.yellow_ramp_handles = []

        self.cube_capacity_per_level = 9*9
        self.corner_capacity_per_level = 9*3        # to be tested
        self.ramp_capacity_per_level = 9*3

        self.vanish_scale = 0.000001

        '''
        0. build all envs
        '''
        # for n in range(self.num_matrix_envs):

        '''
        1. build all actors
        '''
        for i in range(0,self.num_matrix_envs):
            # create environments
            env_ = self.gym.create_env(self.sim, env_lower, env_upper, 4)
            self.envs.append(env_)

            pose = gymapi.Transform()
            pose.p = gymapi.Vec3(0, 0, 0)
            pose.r = gymapi.Quat(0,0,0,1)
            l1_cubes = []
            l2_cubes = []
            l3_cubes = []
            l4_cubes = []
            l5_cubes = []
            l2_corners = []
            l3_corners = []
            l4_corners = []
            l5_corners = []
            l2_ramps = []
            l3_ramps = []
            l4_ramps = []
            l5_ramps = []
            # CUBE
            for j in range(0,self.cube_capacity_per_level):
                gc1 = self.gym.create_actor(self.envs[i], asset_gray_cube, pose, "gray_cube" + str(i) + str(j), i, 0)
                gc2 = self.gym.create_actor(self.envs[i], asset_blue_cube, pose, "blue_cube" + str(i) + str(j), i, 0)
                gc3 = self.gym.create_actor(self.envs[i], asset_yellow_cube, pose, "yellow_cube" + str(i) + str(j), i, 0)
                gc4 = self.gym.create_actor(self.envs[i], asset_red_cube, pose, "red_cube" + str(i) + str(j), i, 0)
                gc5 = self.gym.create_actor(self.envs[i], asset_white_cube, pose, "white_cube" + str(i) + str(j), i, 0)
                l1_cubes.append(gc1)
                l2_cubes.append(gc2)
                l3_cubes.append(gc3)
                l4_cubes.append(gc4)
                l5_cubes.append(gc5)
                self.gym.set_actor_scale(self.envs[i], gc1, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc2, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc3, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc4, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc5, self.vanish_scale)
            # CORNER
            for j in range(0,self.corner_capacity_per_level):
                gc2 = self.gym.create_actor(self.envs[i], asset_blue_corner, pose, "blue_corner" + str(i) + str(j), i, 0)
                gc3 = self.gym.create_actor(self.envs[i], asset_yellow_corner, pose, "yellow_corner" + str(i) + str(j), i, 0)
                gc4 = self.gym.create_actor(self.envs[i], asset_red_corner, pose, "red_corner" + str(i) + str(j), i, 0)
                gc5 = self.gym.create_actor(self.envs[i], asset_white_corner, pose, "white_corner" + str(i) + str(j), i, 0)
                l2_corners.append(gc2)
                l3_corners.append(gc3)
                l4_corners.append(gc4)
                l5_corners.append(gc5)
                self.gym.set_actor_scale(self.envs[i], gc2, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc3, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc4, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc5, self.vanish_scale)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc2, shape_props1)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc3, shape_props1)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc4, shape_props1)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc5, shape_props1)
            # RAMP
            for j in range(0,self.ramp_capacity_per_level):
                gc2 = self.gym.create_actor(self.envs[i], asset_blue_ramp, pose, "blue_ramp" + str(i) + str(j), i, 0)
                gc3 = self.gym.create_actor(self.envs[i], asset_yellow_ramp, pose, "yellow_ramp" + str(i) + str(j), i, 0)
                gc4 = self.gym.create_actor(self.envs[i], asset_red_ramp, pose, "red_ramp" + str(i) + str(j), i, 0)
                gc5 = self.gym.create_actor(self.envs[i], asset_white_ramp, pose, "white_ramp" + str(i) + str(j), i, 0)
                l2_ramps.append(gc2)
                l3_ramps.append(gc3)
                l4_ramps.append(gc4)
                l5_ramps.append(gc5)
                self.gym.set_actor_scale(self.envs[i], gc2, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc3, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc4, self.vanish_scale)
                self.gym.set_actor_scale(self.envs[i], gc5, self.vanish_scale)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc2, shape_props)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc3, shape_props)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc4, shape_props)
                self.gym.set_actor_rigid_shape_properties(self.envs[i], gc5, shape_props)
            # store actors
            self.gray_cube_handles.append(([0],l1_cubes))
            self.blue_cube_handles.append(([0],l2_cubes))
            self.blue_corner_handles.append(([0],l2_corners))
            self.blue_ramp_handles.append(([0],l2_ramps))
            self.yellow_cube_handles.append(([0],l3_cubes))
            self.yellow_corner_handles.append(([0],l3_corners))
            self.yellow_ramp_handles.append(([0],l3_ramps))
            self.red_cube_handles.append(([0],l4_cubes))
            self.red_corner_handles.append(([0],l4_corners))
            self.red_ramp_handles.append(([0],l4_ramps))
            self.white_cube_handles.append(([0],l5_cubes))
            self.white_corner_handles.append(([0],l5_corners))
            self.white_ramp_handles.append(([0],l5_ramps))

            # Agent and Food and Camera

            # generate random x,y coordinates in range [0,18] for the actor
            x = random.uniform(0,16)
            y = random.uniform(0,16)
            facing_ = 0.25 * (float)(random.randint(0,8))
            r_ = gymapi.Quat(0,1,0,1) * gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), facing_*math.pi)

            # create capsule_asset actor in the environment
            pose = gymapi.Transform()
            pose.p = gymapi.Vec3(x, y, self.initial_height)
            pose.r = gymapi.Quat(0,1,0,1)
            cap_handle = self.gym.create_actor(self.envs[i], capsule_asset, pose, 'capsule_asset', i, 0)
            self.gym.set_actor_rigid_shape_properties(self.envs[i], cap_handle, shape_props2)
            self.gym.set_actor_scale(self.envs[i], cap_handle, 0.4)
            self.actor_handles.append(cap_handle)
            self.gym.set_rigid_body_color(self.envs[i], cap_handle, 0, gymapi.MESH_VISUAL_AND_COLLISION, color_red)

            # set random position for food
            x = random.uniform(0,16)
            y = random.uniform(0,16)
            # create capsule_asset food in the environment
            pose = gymapi.Transform()
            pose.p = gymapi.Vec3(x, y, self.initial_height)
            pose.r = gymapi.Quat(0,1,0,1)
            food_handle = self.gym.create_actor(self.envs[i], capsule_asset, pose, 'food_asset', i, 0)
            self.gym.set_actor_rigid_shape_properties(self.envs[i], food_handle, shape_props2)
            self.gym.set_actor_scale(self.envs[i], food_handle, 0.6)
            self.food_handles.append(food_handle)
            self.gym.set_rigid_body_color(self.envs[i], food_handle, 0, gymapi.MESH_VISUAL_AND_COLLISION, color_green)

            att_pose = gymapi.Transform()
            att_pose.p = gymapi.Vec3(x,y, self.initial_height)
            att_pose.r = r_
            attractor_properties_ = gymapi.AttractorProperties()
            attractor_properties_.stiffness = 5e10
            attractor_properties_.damping = 0
            attractor_properties_.axes = gymapi.AXIS_ROTATION
            attractor_properties_.target = att_pose
            attractor_properties_.rigid_handle = cap_handle
            attractor_handle_ = self.gym.create_rigid_body_attractor(self.envs[i], attractor_properties_)

            attractor_properties_food = gymapi.AttractorProperties()
            attractor_properties_food.stiffness = 5e10
            attractor_properties_food.damping = 0
            attractor_properties_food.axes = gymapi.AXIS_ROTATION
            attractor_properties_food.target = pose
            attractor_properties_food.rigid_handle = food_handle
            attractor_handle_food = self.gym.create_rigid_body_attractor(self.envs[i], attractor_properties_food)
            
            h1 = self.gym.create_camera_sensor(self.envs[i], camera_properties)
            camera_offset = gymapi.Vec3(-2, 0, 0)
            camera_rotation = gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 0, 1), 0.4*math.pi)
            body_handle = self.gym.get_actor_rigid_body_handle(self.envs[i], cap_handle, 0)
            self.gym.attach_camera_to_body(h1, self.envs[i], body_handle, gymapi.Transform(camera_offset, camera_rotation), gymapi.FOLLOW_TRANSFORM)
            self.camera_handles.append(h1)
            self.attractor_handles.append(attractor_handle_)

        '''
        2. arrange blocks in the environment
        '''
        for n in range(self.num_matrix_envs):
            self.set_landscape_(n,seeds[n])

        # a must-have operation 
        self.gym.prepare_sim(self.sim)

        self.step_count = 0

        if not self.headless:
            cam_pos = gymapi.Vec3(0.0, 15.0, 15.0)
            cam_target = gymapi.Vec3(1.5, 4, 0)
            self.gym.viewer_camera_look_at(self.viewer, None, cam_pos, cam_target)

        # time test
        self.start_time = time.time()
        self.last_frame_cnt = 0

        # Create helper geometry used for visualization
        # Create an wireframe axis
        self.axes_geom = gymutil.AxesGeometry(2)

        self.facing = 0
        self.facing_step = 0.25

        # local coordinate system
        self.verts = np.empty((3, 1), gymapi.Vec3.dtype)
        self.verts[0][0] = (1, 0, 0)
        self.verts[1][0] = (0, 1, 0)
        self.verts[2][0] = (0, 0, 1)

        # store initial status for all envs
        self.sim_status = []
        for i in range(self.num_matrix_envs):
            env_status = np.copy(self.gym.get_env_rigid_body_states(self.envs[i], gymapi.STATE_ALL))
            self.sim_status.append(env_status)
    
    # reset wfc landscape
    def reset_landscape(self, env_id):
        pose = gymapi.Transform()
        pose.p = gymapi.Vec3(0, 0, 0)
        pose.r = gymapi.Quat(0,0,0,1)

        for i in range(self.cube_capacity_per_level):
            # gray cube
            actor_handle = self.gray_cube_handles[env_id][1][i]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.gray_cube_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # blue cube
            actor_handle = self.blue_cube_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.blue_cube_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # yellow cube
            actor_handle = self.yellow_cube_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.yellow_cube_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # red cube
            actor_handle = self.red_cube_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.red_cube_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # white cube
            actor_handle = self.white_cube_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.white_cube_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

        for i in range(self.corner_capacity_per_level):
            # blue corner
            actor_handle = self.blue_corner_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.blue_corner_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # yellow corner
            actor_handle = self.yellow_corner_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.yellow_corner_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # red corner
            actor_handle = self.red_corner_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.red_corner_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # white corner
            actor_handle = self.white_corner_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.white_corner_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

        for i in range(self.ramp_capacity_per_level):
            # blue ramp
            actor_handle = self.blue_ramp_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.blue_ramp_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)
            
            # yellow ramp
            actor_handle = self.yellow_ramp_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.yellow_ramp_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # red ramp
            actor_handle = self.red_ramp_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.red_ramp_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

            # white ramp
            actor_handle = self.white_ramp_handles[env_id][1][i]
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # reset available block pointer
            self.white_ramp_handles[env_id][0][0] = 0
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, self.vanish_scale)

    # reset wfc landscape interface
    def reset_all_landscape(self):
        for env_id in range(0,self.num_matrix_envs):
            self.reset_landscape(env_id)

    # move block from stash to field
    def arrange_block(self, env_id,block_id,pose):

        if block_id == 1:
            # gray cube
            available_block_id = self.gray_cube_handles[env_id][0][0]
            actor_handle = self.gray_cube_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.gray_cube_handles[env_id][0][0] += 1
            self.gray_cube_handles[env_id][0][0] = min(self.cube_capacity_per_level-1,self.gray_cube_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 2:
            # blue cube
            available_block_id = self.blue_cube_handles[env_id][0][0]
            actor_handle = self.blue_cube_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.blue_cube_handles[env_id][0][0] += 1
            self.blue_cube_handles[env_id][0][0] = min(self.cube_capacity_per_level-1,self.blue_cube_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 3:
            # yellow cube
            available_block_id = self.yellow_cube_handles[env_id][0][0]
            actor_handle = self.yellow_cube_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.yellow_cube_handles[env_id][0][0] += 1
            self.yellow_cube_handles[env_id][0][0] = min(self.cube_capacity_per_level-1,self.yellow_cube_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 4:
            # red cube
            available_block_id = self.red_cube_handles[env_id][0][0]
            actor_handle = self.red_cube_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.red_cube_handles[env_id][0][0] += 1
            self.red_cube_handles[env_id][0][0] = min(self.cube_capacity_per_level-1,self.red_cube_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 5:
            # white cube
            available_block_id = self.white_cube_handles[env_id][0][0]
            actor_handle = self.white_cube_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.white_cube_handles[env_id][0][0] += 1
            self.white_cube_handles[env_id][0][0] = min(self.cube_capacity_per_level-1,self.white_cube_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 6:
            # blue corner
            available_block_id = self.blue_corner_handles[env_id][0][0]
            actor_handle = self.blue_corner_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.blue_corner_handles[env_id][0][0] += 1
            self.blue_corner_handles[env_id][0][0] = min(self.corner_capacity_per_level-1,self.blue_corner_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 7:
            # yellow corner
            available_block_id = self.yellow_corner_handles[env_id][0][0]
            actor_handle = self.yellow_corner_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.yellow_corner_handles[env_id][0][0] += 1
            self.yellow_corner_handles[env_id][0][0] = min(self.corner_capacity_per_level-1,self.yellow_corner_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 8:
            # red corner
            available_block_id = self.red_corner_handles[env_id][0][0]
            actor_handle = self.red_corner_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.red_corner_handles[env_id][0][0] += 1
            self.red_corner_handles[env_id][0][0] = min(self.corner_capacity_per_level-1,self.red_corner_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 9:
            # white corner
            available_block_id = self.white_corner_handles[env_id][0][0]
            actor_handle = self.white_corner_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.white_corner_handles[env_id][0][0] += 1
            self.white_corner_handles[env_id][0][0] = min(self.corner_capacity_per_level-1,self.white_corner_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 10:
            # blue ramp
            available_block_id = self.blue_ramp_handles[env_id][0][0]
            actor_handle = self.blue_ramp_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.blue_ramp_handles[env_id][0][0] += 1
            self.blue_ramp_handles[env_id][0][0] = min(self.ramp_capacity_per_level-1,self.blue_ramp_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 11:
            # yellow ramp
            available_block_id = self.yellow_ramp_handles[env_id][0][0]
            actor_handle = self.yellow_ramp_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.yellow_ramp_handles[env_id][0][0] += 1
            self.yellow_ramp_handles[env_id][0][0] = min(self.ramp_capacity_per_level-1,self.yellow_ramp_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 12:
            # red ramp
            available_block_id = self.red_ramp_handles[env_id][0][0]
            actor_handle = self.red_ramp_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.red_ramp_handles[env_id][0][0] += 1
            self.red_ramp_handles[env_id][0][0] = min(self.ramp_capacity_per_level-1,self.red_ramp_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)
        elif block_id == 13:
            # white ramp
            available_block_id = self.white_ramp_handles[env_id][0][0]
            actor_handle = self.white_ramp_handles[env_id][1][available_block_id]
            actor_state = self.gym.get_actor_rigid_body_states(self.envs[env_id], actor_handle, gymapi.STATE_ALL)
            actor_state['pose']['p'].fill((pose.p.x,pose.p.y,pose.p.z))
            actor_state['pose']['r'].fill((pose.r.x,pose.r.y,pose.r.z,pose.r.w))
            self.gym.set_actor_rigid_body_states(self.envs[env_id], actor_handle, actor_state, gymapi.STATE_POS)
            # integrate available block pointer
            self.white_ramp_handles[env_id][0][0] += 1
            self.white_ramp_handles[env_id][0][0] = min(self.ramp_capacity_per_level-1,self.white_ramp_handles[env_id][0][0])
            self.gym.set_actor_scale(self.envs[env_id], actor_handle, 1)

    # set wfc landscape
    def set_landscape_(self, env_id, seed_):

        interval_xy = 2
        interval_z = 2*0.7
        # ramp and corner attitutde
        ramp_rot0 = gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 0*math.pi)
        ramp_rot1 = gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 0.5*math.pi)
        ramp_rot2 = gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 1*math.pi)
        ramp_rot3 = gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 1.5*math.pi)
        corner_rot0 = gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 0*math.pi)
        corner_rot1 = gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 0.5*math.pi)
        corner_rot2 = gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 1*math.pi)
        corner_rot3 = gymapi.Quat.from_axis_angle(gymapi.Vec3(0, 1, 0), 0.5*math.pi) * gymapi.Quat.from_axis_angle(gymapi.Vec3(1, 0, 0), 1.5*math.pi)

        # create actors based on WFC seed
        for i in range(0,9):
            for j in range(0,9):
                tile_ = seed_.wave_oriented[j*9+i][0][0]
                rot = seed_.wave_oriented[j*9+i][0][1]
                pose_ij = gymapi.Transform()
                pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 0*interval_xy)
                pose_ij.r = gymapi.Quat(0,0,0,1)
                self.arrange_block(env_id, 1, pose_ij)
                # cubes
                if tile_<=5:
                    if tile_==1:
                        pass
                    elif tile_==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        self.arrange_block(env_id, 2, pose_ij)
                    elif tile_==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        self.arrange_block(env_id, 2, pose_ij)
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                        self.arrange_block(env_id, 3, pose_ij)
                    elif tile_==4:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        self.arrange_block(env_id, 2, pose_ij)
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                        self.arrange_block(env_id, 3, pose_ij)
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        self.arrange_block(env_id, 4, pose_ij)
                    elif tile_==5:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        self.arrange_block(env_id, 2, pose_ij)
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                        self.arrange_block(env_id, 3, pose_ij)
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        self.arrange_block(env_id, 4, pose_ij)
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        self.arrange_block(env_id, 5, pose_ij)
                # corners
                elif tile_==6:
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = corner_rot2
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = corner_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = corner_rot0
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = corner_rot3
                    self.arrange_block(env_id, 6, pose_ij)
                elif tile_==7:
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                    self.arrange_block(env_id, 2, pose_ij)
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                        pose_ij.r = corner_rot2
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                        pose_ij.r = corner_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                        pose_ij.r = corner_rot0
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                        pose_ij.r = corner_rot3
                    self.arrange_block(env_id, 7, pose_ij)
                elif tile_==8:
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                    self.arrange_block(env_id, 2, pose_ij)
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                    self.arrange_block(env_id, 3, pose_ij)
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = corner_rot2
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = corner_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = corner_rot0
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = corner_rot3
                    self.arrange_block(env_id, 8, pose_ij)
                elif tile_==9:
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                    self.arrange_block(env_id, 2, pose_ij)
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                    self.arrange_block(env_id, 3, pose_ij)
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                    self.arrange_block(env_id, 4, pose_ij)
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = corner_rot2
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = corner_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = corner_rot0
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = corner_rot3
                    self.arrange_block(env_id, 9, pose_ij)
                # ramps
                elif tile_==10:
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot0
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot2
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot3
                    self.arrange_block(env_id, 10, pose_ij)
                elif tile_==11:
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                    self.arrange_block(env_id, 2, pose_ij)
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot0
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot2
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                        pose_ij.r = ramp_rot3
                    self.arrange_block(env_id, 11, pose_ij)
                elif tile_==12:
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                    self.arrange_block(env_id, 2, pose_ij)
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                    self.arrange_block(env_id, 3, pose_ij)
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = ramp_rot0
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = ramp_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = ramp_rot2
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                        pose_ij.r = ramp_rot3
                    self.arrange_block(env_id, 12, pose_ij)
                elif tile_==13:
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 1*interval_z)
                    self.arrange_block(env_id, 2, pose_ij)
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 2*interval_z)
                    self.arrange_block(env_id, 3, pose_ij)
                    pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 3*interval_z)
                    self.arrange_block(env_id, 4, pose_ij)
                    if rot==0:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = ramp_rot0
                    elif rot==1:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = ramp_rot1
                    elif rot==2:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = ramp_rot2
                    elif rot==3:
                        pose_ij.p = gymapi.Vec3(i*interval_xy, j*interval_xy, 4*interval_z)
                        pose_ij.r = ramp_rot3
                    self.arrange_block(env_id, 13, pose_ij)

    # set wfc landscape interface
    def set_landscape(self, env_id, seed_):
        self.reset_landscape(env_id)
        self.set_landscape_(env_id,seed_)

    @property
    def num_envs(self):
        return self.num_matrix_envs


    def calculate_reward(self, env_, actor_handle, food_handle):

        return 0

    def done_determinant(self, env_, actor_handle, food_handle, reward = 0, step = 0):

        return False

    def step(self, action : int, physics_only = False):

        self.step_count += 1

        observation = np.zeros((HEIGHT,WIDTH, 4), np.uint8)

        '''
        step the physics
        '''
        self.gym.simulate(self.sim)
        self.gym.fetch_results(self.sim, True)
        if not self.headless:
            self.gym.clear_lines(self.viewer)
        self.gym.step_graphics(self.sim)

        if not physics_only:

            self.gym.render_all_camera_sensors(self.sim)

            '''
            get observation
            '''
            for i in range(self.num_matrix_envs):
                rgb_image = self.gym.get_camera_image(self.sim, self.envs[i], self.camera_handles[i], gymapi.IMAGE_COLOR)
            # # create an opencv image of size (HEIGHT,WIDTH , 4)
            
            # observation[:, :, :4] = rgb_image.reshape((HEIGHT,WIDTH, 4))
            # rotate 90 degrees counter-clockwise
            observation = np.rot90(observation, -1)
            # transform the image from RGBA to RGB
            observation = observation[:, :, :3]

            self.camera_sensor_image = np.copy(observation)

        else:
            observation = np.zeros((HEIGHT,WIDTH, 3), np.uint8)

        '''
        get reward
        '''
        reward = 0 #self.calculate_reward(self.envs[i], self.actor_handles[i], self.food_handles[i])
        done = 0 #self.done_determinant(self.envs[i], self.actor_handles[i], self.food_handles[i],reward,self.step_count)
        info = {}

        # if self.show_action:
        #     self.show_image()
        
        # if not self.headless:
        #     # update the viewer
        #     self.gym.draw_viewer(self.viewer, self.sim, True)

        end_time = time.time()
        if (end_time - self.start_time) > 1:
            print("FPS: %.2f" % ((self.step_count - self.last_frame_cnt)*self.num_matrix_envs))
            self.last_frame_cnt = self.step_count
            self.start_time = time.time()
        
        return np.copy(observation), reward, done, info

    def reset(self):

        self.step_count = 0
        self.facing = 0
        self.start_time = time.time()

        observation, reward, done, info = self.step(action = -3, physics_only = False)
        self.camera_sensor_image = np.copy(observation)

        return np.copy(observation)

    def render(self, mode='human', real_time = False):
        
        # body_states = self.gym.get_actor_rigid_body_states(self.envs[i], self.actor_handles[i], gymapi.STATE_ALL)
        # actor_p = body_states['pose']['p']
        # # print("actor position: (%f, %f, %f)" % (actor_p[0][0], actor_p[0][1], actor_p[0][2]))
        # cnnc_x = ((actor_p[0][0]+1)/(9*2))*20*9
        # cnnc_y = ((actor_p[0][1]+1)/(9*2))*20*9

        # print(self.connectivity_map[int(cnnc_y)][int(cnnc_x)])

        if not self.headless:

            if self.gym.query_viewer_has_closed(self.viewer):
                return -2
            # update the viewer
            self.gym.draw_viewer(self.viewer, self.sim, True)

            # # for i in range(self.num_matrix_envs):
            # body_states = self.gym.get_camera_transform(self.sim, self.envs[i], self.camera_handles[i])
            # draw_camera(self.gym, self.viewer,self.envs[i],body_states)
            # # self.gym.draw_env_rigid_contacts(self.viewer, self.envs[i], gymapi.Vec3(1,0,0), 2, True)
            

            # if real_time:
            #     self.gym.sync_frame_time(self.sim)

            # self.camera_sensor_image = cv2.cvtColor(self.camera_sensor_image, cv2.COLOR_RGB2BGR)
            # cv2.imshow('rgb', self.camera_sensor_image)

            # # draw dot at [cnnc_x, cnnc_y]
            # cnnc_vis = np.copy(self.connectivity_map_visual)
            # cv2.circle(cnnc_vis, (int(cnnc_x), int(cnnc_y)), 5, (0, 0, 255), -1)
            # cv2.imshow('conn',cnnc_vis)
            # self.interaction = cv2.waitKey(1)
            
        return self.interaction

    def show_image(self):
        
        i = 0
        body_states = self.gym.get_actor_rigid_body_states(self.envs[i], self.actor_handles[i], gymapi.STATE_ALL)
        actor_p = body_states['pose']['p']
        # print("actor position: (%f, %f, %f)" % (actor_p[0][0], actor_p[0][1], actor_p[0][2]))
        cnnc_x = ((actor_p[0][0]+1)/(9*2))*20*9
        cnnc_y = ((actor_p[0][1]+1)/(9*2))*20*9

        self.camera_sensor_image = cv2.cvtColor(self.camera_sensor_image, cv2.COLOR_RGB2BGR)
        cv2.imshow('rgb', self.camera_sensor_image)
        cv2.waitKey(1)

    def close (self):
        
        if not self.headless:
            self.gym.destroy_viewer(self.viewer)

        self.gym.destroy_sim(self.sim)
